# simulate udp send

import struct 
import socket 
import logging
from logging.config import dictConfig
import yaml
import os
import time
from math import pi, cos


# 计算程序文件所在的实际目录，文件名和无扩展的文件名
real_path = os.path.realpath(__file__)
logging.info('real_path = {}'.format(real_path))
base_name = os.path.basename(real_path)
logging.info('base_name = {}'.format(base_name))
base_dir = os.path.dirname(real_path)
logging.info('base_dir = {}'.format(base_dir))
os.chdir(base_dir)
logging.info('change dir to base_dir = {}'.format(os.getcwd()))
program_name,_ext = os.path.splitext(base_name)
config_file = os.path.join(base_dir,program_name+'.yaml') #计算配置文件
logging.info('config_file = {}'.format(config_file))

import yaml
with open(config_file,'r',encoding='utf-8') as file:
    config = yaml.load(file,Loader=yaml.Loader)
    dictConfig(config['logging'])
    logger = logging.getLogger()
    logger.info('config:')
    logger.info(config)


def main(main_config):
    target_ip = main_config['target_ip']
    target_udp_port = main_config['target_udp_port']
    source_udp_port = main_config['source_udp_port']
    delay = main_config['delay']
    start_time  = time.time()*1e6

    client_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    client_socket.bind(("",source_udp_port))
    target_addr = (target_ip, target_udp_port)
    
    while(True):
        try:
            #'time_boot_us,lat,lon,alt,homelat,homelon,homealt,vx,vz,distance,vol,roll,pitch,yaw'
            udp_msg_formatter= '<IiiiiiihhhHfff'
            buf = bytearray(struct.calcsize(udp_msg_formatter)+8)
            time_boot_us =  time.time()*1e6 -start_time
            tiananmen= (39.907375,116.391349,80) # lat,lon,alt in meters
            jichang = (39.968598,116.265292,100)
            centers = [0,*tiananmen,*jichang,0,0,1,20,0,0,0]
            amplitudes = [0,0.001,0.001,10,0,0,10,5,5,0.2,1,0.1,0,1,0.1]
            phases = list(range(len(centers))) # 度
            periods = [30]*len(centers) # 周期,s
            data_types = udp_msg_formatter[1:]

            time_in_sec = time_boot_us/1e6
            logger.debug(f'time_in_sec={time_in_sec}')

            physical_values = [b+r*cos(time_in_sec/T*2*pi+p*pi/180)for b,r,T,p in zip(centers,amplitudes,periods,phases)]
            physical_values[0] = time_in_sec*1e6
            names = 'time_boot_us,lat,lon,alt,homelat,homelon,homealt,vx,vz,distance,vol,roll,pitch,yaw'.split(',')
            units = 'us,deg,deg,m,deg,deg,m,m/s,m/s,m,V,rad,rad,rad'.split(',')
            logger.debug(list(zip(names,physical_values,units,data_types)))

            scalings = [1,1e7,1e7,1e3,1e7,1e7,1e3,1e2,1e2,1e2,1e3,1,1,1]
            offsets = [0]*len(names)
            udp_values  = [int(s*(v-o)) for v,s,o in zip(physical_values,scalings,offsets)]
            # logger.info([names,physical_values,units,data_types])
            logger.info(list(zip(data_types,udp_values)))

            struct.pack_into(udp_msg_formatter,buf,6,*udp_values)

            logger.debug(buf)
            client_socket.sendto(buf,target_addr)
            time.sleep(delay)
        except KeyboardInterrupt:
            logger.info('KeyboardInterrupt')
            break
        except Exception as e:
            logger.exception(e)
            continue
if __name__ == "__main__":
    main(config['main'])